import numpy as np
from PIL import Image
import os
import sys
import random
project_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))
sys.path.insert(0, project_root)

from utils.rotate import rotate
from data.verify_utils import depthmap_to_absolute_camera_coordinates


def verify():
    """Main verification function using pixel-to-pixel pointmap comparison."""
    print("--- Starting Verification (Pixel-to-Pixel Method) ---")
    np.random.seed(22)

    # 1. Generate Mock Data
    print("1. Generating mock data...")
    H, W = 640, 480
    # Create a mock image. The content doesn't matter for this verification.
    # image_np = np.zeros((H, W, 3), dtype=np.uint8)
    # image = Image.fromarray(image_np)
    image = np.random.rand(H, W, 3)
    depth = np.random.rand(H, W)
    print(f"--- Generated image with seed. Top-left pixel: {image[0, 0]} ---") 

    # Create a depth map with a gradient to ensure all values are unique
    depth = np.fromfunction(lambda j, i: (j / H) + (i / W) + 1.0, (H, W), dtype=np.float32)
    intrinsics = np.array([[436.11008, 0., 240.91835],
                       [0., 436.11008, 322.66653],
                       [0., 0., 1.]])

# Original Camera Extrinsics Matrix (Pose) [R | t] - World to Camera
    pose = np.array([[ 9.8153263e-01, -7.7415816e-02,  1.7492986e-01, -6.1428569e-02],
                          [ 7.7405863e-02,  9.9697584e-01,  6.8903100e-03, -4.0515000e-04],
                          [-1.7493425e-01,  6.7775301e-03,  9.98455679e-01,  1.0289070e-02], # Using corrected value
                          [ 0.0000000e+00, -0.0000000e+00, -0.0000000e+00,  1.0000000e+00]])
    # # Define realistic camera intrinsics
    # intrinsics = np.array([
    #     [50.0, 0.0, W / 2.0],  # [fx,  0, cx]
    #     [0.0, 50.0, H / 2.0],  # [ 0, fy, cy]
    #     [0.0, 0.0, 1.0]
    # ], dtype=np.float32)
    
    # pose = np.array([
    #     [1.0, 0.0, 0.0, 0.5],
    #     [0.0, 1.0, 0.0, 1.0],
    #     [0.0, 0.0, 1.0, 1.5],
    #     [0.0, 0.0, 0.0, 1.0]
    # ], dtype=np.float32)
    print(f"   Original Dimensions: H={H}, W={W}")

    # 2. Unproject BEFORE rotation
    print("2. Unprojecting original data to create ground truth 3D pointmap...")
    # This now calls the imported function
    points_before, _, _ = depthmap_to_absolute_camera_coordinates(depth, intrinsics, pose)
    print(f"   Generated pointmap of shape: {points_before.shape}")

    # 3. Apply the rotation
    print("3. Applying the `rotate` function...")
    # This now calls the imported function
    image_rot, depth_rot, intrinsics_new, pose_new, _ = rotate(image, depth, intrinsics, pose)
    print(f"   New Rotated Dimensions: H'={image_rot.shape[0]}, W'={image_rot.shape[1]}")

    # 4. Unproject AFTER rotation
    print("4. Unprojecting rotated data to create new 3D pointmap...")
    points_after, _, _ = depthmap_to_absolute_camera_coordinates(depth_rot, intrinsics_new, pose_new)
    print(f"   Generated pointmap of shape: {points_after.shape}")

    # 5. Re-align the 'after' pointmap for direct comparison
    # We rotate it 90 degrees clockwise (k=-1 or 3 times CCW) to undo the original CCW rotation.
    print("5. Re-aligning rotated pointmap for comparison...")
    points_after_realigned = np.rot90(points_after, k=-1)
    print(f"   Re-aligned pointmap to shape: {points_after_realigned.shape}")

    # 6. Compare the pointmaps pixel-to-pixel using a tolerance
    print("6. Verifying the pointmaps are identical...")
    are_identical = np.allclose(points_before, points_after_realigned, atol=1e-6)

    print("\n--- Verification Result ---")
    if are_identical:
        print("✅ SUCCESS: The 3D pointmaps are identical after re-alignment.")
        print("The `rotate` function is verified to be correct.")
    else:
        print("❌ FAILURE: The 3D pointmaps do NOT match.")
        # Find and print the location and value of the maximum difference for debugging
    diff = np.abs(points_before - points_after_realigned)
    max_diff_idx = np.unravel_index(np.argmax(diff), diff.shape)
    print(f"\nExample of mismatch at pixel {max_diff_idx[:2]}:")
    print(f"Point Before:           {points_before[max_diff_idx[0], max_diff_idx[1]]}")
    print(f"Point After (re-aligned): {points_after_realigned[max_diff_idx[0], max_diff_idx[1]]}")
    print(f"Absolute Difference:      {diff[max_diff_idx[0], max_diff_idx[1]]}")


if __name__ == '__main__':    
    verify() # Commented out to prevent import errors in environments without the dependencies.
